package com.googlecode.grs.environment;

import java.awt.Graphics;
import java.awt.geom.Point2D;
import java.util.ArrayList;

import com.googlecode.grs.robot.Moveable;
import com.googlecode.grs.ui.Drawable;

/**
 * Environment consists of all objects in the world, Along with their placements
 * and the placement of the robot. All interactions in world occur based on this
 * data
 * 
 * This is constructed from a maze file. While it makes more sense to separate
 * the Moveable and the physical objects, a maze file is a specification of a
 * simulation environment. Robot (starting) position is not separate.
 * 
 * An Environment thus describes a SimulationEnvironment, which specifies a maze
 * and starting robot position.
 * 
 * 
 * @author kroo, ajc
 */

public class SimulationEnvironment implements Drawable {
	// list of all objects in the environment
	private ArrayList<PhysicalObject> objects;
	// robot in the maze.
	// TODO consider multiple robots and how that wrecks things
	private Moveable robot;

	/**
	 * Ctor called by this class's factory. The Moveable robot component needs
	 * only a position to be constructed
	 * 
	 * 
	 * 
	 * @see Moveable
	 */
	SimulationEnvironment() {
		objects = new ArrayList<PhysicalObject>();
	}

	/**
	 * Add an object to the environment.
	 * 
	 * @param o
	 *            the object to be added
	 * @see PhysicalObject
	 */
	public void add(PhysicalObject o) {
		objects.add(o);
	}

	/**
	 * Get the i'th object in the environment (convenience method)
	 * 
	 * @return the i'th object in the environment
	 * @param i
	 *            index of the object
	 */
	public PhysicalObject get(int i) {
		return objects.get(i);
	}

	/**
	 * Retrieves the array of PhysicalObjects in the environment
	 * 
	 * @return every object in the environment
	 */
	public ArrayList<PhysicalObject> getObjects() {
		return objects;
	}

	/**
	 * Removes an object from the environment
	 * 
	 * @param o
	 *            the object to be removed
	 */
	public void remove(PhysicalObject o) {
		objects.remove(o);
	}

	// These are local objects for ranged sensors like sonar
	/**
	 * NOTE This method will return all known local objects, which simulated
	 * sensors will need to filter by the actual reach of the hardware
	 */
	public ArrayList<PhysicalObject> getLocalObjects(double x, double y,
			double radius) {

		/*
		 * ArrayList<PhysicalObject> locals = new ArrayList<PhysicalObject>();
		 * for(int i=0; i<objects.size(); i++) { PhysicalObject local =
		 * objects.get(i);
		 * 
		 * if(Math.sqrt( (x - local.getX()) * (x - local.getX()) + (y -
		 * local.getY()) * (y - local.getY()) ) <= radius){
		 * 
		 * locals.add(local); }
		 * 
		 * }
		 */
		return objects;
	}

	/**
	 * 
	 * @return robot that tracks and maintains position
	 */
	public Moveable getRobot() {
		return robot;
	}

	/**
	 * 
	 * @return X component of robot's position
	 */
	public double getRobotX() {
		return robot.getX();
	}

	/**
	 * 
	 * @return Y component of robot's position
	 */
	public double getRobotY() {
		return robot.getY();
	}

	/**
	 * 
	 * @return robot's current orientation (right = 0, ccw, units are radians)
	 */
	public double getRobotHeading() {
		return robot.getHeading();
	}

	/**
	 * 
	 * @deprecated robot setposition is questionable
	 * @param position
	 * @param angle
	 */
	public void setRobotPosition(Point2D.Double position, double angle) {
		robot.setPosition(position, angle);
	}

	/**
	 * Paints all physical objects in the environment (not including robot)
	 */
	public void draw(Graphics g) {
		for (PhysicalObject o : objects) {
			o.draw(g);
		}
	}

	/**
	 * Adds this robot to be accessed by clients of this class
	 * 
	 * @param robot
	 *            A MoveableObject(robot)
	 */
	public void addRobot(Moveable robot) {
		this.robot = robot;
	}
}